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ABSTRACT. This paper presents algorithms for computing constraints on the position of an object 
due to the presence of obstacles. This problem arises in applications which require choosing how 
to arrange or move objects among other objects. The basis of the approach presented here is to 
characterize the position and orientation of the object of interest as a single point in a Configuration 
Space, in which each coordinate represents a degree of freedom in the position and/or orientation 
of the object. The configurations forbidden to this object, due to the presence of obstacles, can then 
be characterized as regions in the Configuration Space. The paper presents algorithms for computing 
these Configuration Space obstacles when the objects and obstacles are polygons or polyhedra. An 
approximation technique for high-dimensional Configuration Space obstacles, based on projections of 
obstacles slices, is described. 
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I nl rod uct ion 


1. Introduction 


An increasing range of computer applications deal with models of two- and three-dimensional 
objects. In these applications, objects must often be placed among other objects or moved in such 
a way so that no interference from nearby objects result. In this paper, these types of problems 
are called spatial planning problems. Among the many applications where spatial planning plays an 
important role are: 

1. Planning the layout of a building [8], i.e. the arrangement of walls, corridors, rooms, and 
equipment so as to fulfill a user’s design constraints as well as implicit consistency constraints. 

2. Planning how to machine a part using a Numerically Controlled Machine Tool [47], which 
requires plotting the path of one or more cutting surfaces so as to produce the desired part. 


3. Planning the layout of an 1C chip [45] so as to minimize area, subject to geometric design 
constraints. 

4. Planning how to assemble a part using an industrial robot [18] [19] [38], which requires 
choosing how to grasp objects, move them without collisions and bring them into contact. 


Problems in spatial planning generally involve (at least) two important types of considerations: 

1. Geometric — The legal solutions must be characterized, which involves considering inter¬ 
actions between the shapes of objects and obstacles. 


2. Optimization — The best solution must be chosen from among tire legal solutions. 


This paper deals primarily with computing constraints on the position of an object due to the presence 
of obstacles, thus its focus is on the geometric aspect of spatial planning. The development throughout 
will be based on polyhedral object models, although many of die results and approaches are ap¬ 
plicable to other classes of object models. 

Recently, diere has been a rapid growth of interest in efficient algoridims for geometric problems. 
Previous work 1 has focused on algorithms for (1) computing convex hulls [9] [12] [15] [29], (2) inter¬ 
secting convex polygons and polyhedra [5] [24] [33] [35], (3) intersecting half-spaces [7] [30] (4) decom¬ 
posing polygons [32], and (5) closcst-point problems [34], This paper formulates two other geometric 

’The references cited here are representative of the current literature; they are by no means a complete survey. 
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Spatial Planning Problems 


problems, Findspace and Findpath, with important applications in spatial planning, describes an 
approach to their solution and presents algorithms for the central problem posed by the approach. 
Previous work on these problems is briefly reviewed in Section 11. 


2. Spatial Planning Problems 

Let R be a convex polyhedron that contains kn other, possibly overlapping, convex polyhedra Bj 
designated as obstacles. Let A be the union of k A convex polyhedra A,, i.e. A — Figure 

1 illustrates two related geometric problems, defined below (where position is used to mean both 
translation and orientation): 

1. Findspace — Find a position for A, inside R , such that VWj \ A l C\B J = 0, This is called a 
safe position. 

2. Findpath — Find a path for A from position s to position g such that A is always in R and A 
never overlaps any of die B,. This is called a safe path. 

Versions of the Findpath and Findspace problems occur in many spatial planning applications. 
For example, the approaches to object layout (template packing) in [1] [2] [3] and [10] are based on 
solutions to Findspace in two-dimensions. Also, systems for programming industrial robots using 
object models [20] [38] must solve 3-dimcnsional Findspace and Findpath problems when choosing 
grasp points on objects or paths for the robot. 


3. The Cspace Approach to Spatial Planning 

In this section, an overview of the Configuration Space approach to spatial planning will be 
presented. Sections 5 through 10 discuss the approach more formally. 

The position and orientation of a rigid solid can be represented as a single 6-dimcnsional point, 
called its configuration. The 6-dimensional space of configurations for a solid. A, is called its 
Configuration Space and denoted Cspace A . For example, a configuration may have one coordinate 
value for each of the x, y, z coordinates of a selected point on the object and one coordinate value for 




4 


The Ctiixia Approach lo Spatial Planning 






Figure 1 . The definition of R, B, and A for Findspace and Findpath problems in two dimen¬ 
sions. (a) The Findspace problem is to find a position for A which docs not overlap any of the 
By (b) The Findspace problem is to find a path for 4 from s to g that avoids tlieB,-. 
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The Cspac< Approach to Spatial Planning 



each of the object’s Euler angles 2 . In general, an n-dimcnsional configuration space can be used to 
model any system that can be characterized with n parameters. An example is the configuration of 
an industrial robot with n joints, where n is typically 5 or 6. In Cspace A , die set of configurations 
where A overlaps B will be denoted CO a (B), the Cspace A Obstacle due to B. Similarly, those 
configurations where A is completely inside B will be denoted CT A (B ), the Cspace A Interior of B. 
Together, these two Cspace A constructs embody all die information needed to solve Findspace and 
Findpath problems. 

If the orientation of a convex polygon A is fixed, Cspace A is simply the (x, y ) plane. This 
is so because the ( x, y) position of some reference vertex rv A is sufficent to specify the polygon’s 
configuration. In diis case, the presence of another convex polygon B constrains rv A to be outside 
of CO a [B ), a larger convex polygon, shown as die shaded region in Figure 2. Thus, the Findspace 
problem can be transformed to the equivalent problem of placing rv A outside of CO A (B), but inside 
CI a {R). Similarly, for multiple obstacles j 3„ a location for A is safe iff rv A is not inside any of the 

2 The relative rotation of one coordinate system relative to another can be specified in terms of three angles usually 
referred to as Euler angles [43], These angles indicate the magnitude of three successive rotations about specified axes, 
but no uniform convention for the choice of axes exists. 
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The C'spare Approach lo Spatial Planning 



Figure 3. The Findpath problem and its formulation using the The shortest collision-free paths 

connect the origin and the destination via the vertices of the CO polygons. 


CO A (B t ), but is inside CI A (R). Subsequent sections discuss algorithms for CO A (B) and CI A (B). 

If the orientation of A is fixed, then die Findpath problem for the polygon A among die B, is 
equivalent to die Findpath problem for die point rv A among the CO A (B,). When die CO A {Bi) are 
polygons, the shortest 3 safe paths for rv A arc piecewise linear paths connecting die start and the 
goal via the vertices of the CO polygons, Figure 3. Therefore, Findpath can be formulated as a 
graph search problem. The graph is formed by connecting all pairs of CO vertices (and the start 
and goal) that can "see" each other, i.e. can be connected by a straight line diat does not intersect 
any of the obstacles. The shortest padi from die start to the goal in diis visibility graph (Vgraph) is 
die shortest safe padi for A among die B t [21]. This algorithm efficiently solves Findpath problems 
when the orientation of A is fixed, but the padis it finds are very susceptible to inaccuracies in the 
object model. These padis touch the Cspace A obstacles, diercforc if the model were exact, an object 
moving along diis type of path would just touch die obstacles. But an inaccurate model may result in 
a collision. Furthermore, the Vgraph algorithm with dircc-dimensional objects and obstacles does not 
find optimal paths. 


3 This assumes Euclidean distance as a metric. For the optimality conditions using a rectilinear (Manhattan) metric see 
[16]. 
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The C'sjxH'i Approach to Spatial Planning 



When A is a three-dimensional solid which is allowed to rotate, CO\{B) is a complicated curved 
object in a 6 -dimensional Cspace A . Rather than compute these objects directly, the approach taken 
here is to use a sequence of two- and three-dimensional projections of the high-dimensional Cspace A 
obstacles. In particular, the 6 -dimcnsional Cspace A obstacles for a rigid solid can be approximated 
by several 3-dimcnsional projections of CO slices. A /-slice of an object C £ 3? n is defined to be 
{(A, • • • > Ai) £ C | 7 j < (3j <. ')'■}, where 7 y and 7 '. are the lower and upper bounds of the slice, 
respectively. Then, if A" is a set of indices between 1 and n, a A'-slice is the intersection of all the j- 
slices for j £ K. Notice that a A'-slice of C is an object of the same dimension as C. Slices can then 
be projected onto those coordinates not in K to obtain objects of lower dimension. 

As an example of the slice projection technique, Figure 4 shows, shaded, the (x, y) projection of 
0-slices of CO a {B ) when A and B are convex polygons. These slices represent configurations where 
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A overlaps B for some orientation of A in the specified range of 0. Section 10 shows that these slice 
projections arc the Cspace \ obstacles of the area (volume) swept out by A over the range of orienta- 
lions of die slice. Note that approximating the swept volume as a polyhedron leads to a polyhedral 
approximation for die projected slices, as shown in Figure 4. 

The slice projection technique has two important properties: 

1. A solution to a Findspace problem in any of the slices is a solution to the original problem, 
but since the slices are an approximation to the Cspace obstacle, die converse is not necessarily 
true. 

2. The slice projection of Cspace A obstacle can be computed without having to compute die 
high-dimensional Cspace A obstacle, see Section 10. 

The slice projection method can also be used to extend the Vgraph algorithm described earlier to 
find safe (but sub-optimal) padis when rotations of A are allowed [21]. A number of slice projections 
of the Cspace obstacles arc constructed for different ranges of orientations of A. The problem of 
planning safe paths in the high-dimensional Cspace A is decomposed into (a) planning safe paths 
via CO vertices within each slice projection and (b) moving between slices, at configurations that 
arc safe in both slices. Both of diese types of motions can be modelled as links in die Vgraph, 
therefore the complete algoridim can be formulated as a graph search problem. This approach is 
illustrated in Figure 5. However, the Vgraph algoridim has several drawbacks when die obstacles are 
3-dimensional. In particular, 

1. Optimal padis in higher dimensions do not typically traverse the vertices of the Cspace 
obstacles. 

2. In higher dimensions, diere may be no paths via vertices, within the enclosing polyhedral 
region R, although other types of safe padis within R may exist. 

These drawbacks may be alleviated by introducing additional nodes in the Vgraph which do not 
correspond to vertices [21]. An alternative strategy to finding safe paths in Cspace A is discussed in 
[ 20 ]. 

The key to the Cspace approach outlined above is computing the Cspace obstacles; the rest of 
the paper is devoted to this problem. 
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Notation and Conventions 




Figure 5. An illustration of the Findpath algorithm using slice projection described by I ozano-Perez and 
Wesley in [21]. A number of slice projections of the Cspace obstacles are constructed for dilTerent ranges 
of orientations of A. 'Hie problem of planning safe paths in die high-dimensional Cspace A is decomposed 
into (I) planning safe paths via CO vertices within each slice projection and (2) moving between slices, at 
configurations that arc safe in both slices. A\ represents \ in its initial configuration. A 3 represents A in its 
final configuration and A 2 is a simple polyhedral approximation to the swept volume of A between its initial 
and final orientation. 


4. Notation and Conventions 


All geometric entities — points, lines, edges, planes, faces and objects — will be treated as 
(infinite) sets of points. All of these entities will be in 3? fe , the fc-dimcnsional real Euclidean space, o, 
b, x, and y shall denote points of U k \ as well as the corresponding vectors. A, B, and C shall denote 
sets of points in 3^', while I and K shall denote sets of integers. 7 , 0, and 0, shall denote reals, while 
n, i,j, k shall be used for integers. The coordinate representation of a point c G 5? n , for any n, shall 
be c = ( 7 ,;) = ( 71 ,..., 7 n ). The magnitude of a vector a will be ||a|| and the cardinality of a set A 
will be |/4|. The scalar (dot) product of vectors a and b will be denoted (a, b). 

The following operations are defined on sets of points in 5R n : . 
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Figure 6. I inked Polyhcdra can be used to model the gross geometry of manipulators. 


A@B = {a + b\aEA,b£B} 

0/1 = { — a | a E A } 

If a seta consists of a single point a, then a® B — (a}©B = A©B. Also, A ©B = a©(©B). 
Note that, typically A ® A {2a \ a £ A} and A © A jA 0, although A © B = B ® A. The set 
difference and set complement operations will be denoted a — B and — A respectively. 

Rigid objects will be represented as sets of possibly overlapping convex polyhedra since this repre¬ 
sentation simplifies the algorithms for computing CO. Theorem 1 below follows directly from the 
definition of CO; it justifies the use of this object representation. 

Theorem 1: Ifa = \JlL\Ai andB = (J ) B =l Bf 


CO a (B ) =00 c °aXB:). 

2 = 1 j = 1 

I he position and orientation of a polyhedron will be defined relative to an initial position and 
orientation. In this initial position, some vertex of the polyhedron coincides with the origin of the 
global coordinate frame. For a polyhedron P, this vertex is called the reference vertex ofP , or rv P . 

In the sequel, a different kind of object, called Linked polyhedra , is used. These objects are 
kinematic chains with polyhedral links and prismatic or rotary joints 4 Figure 6. The relative position 

4 Joints arc represented abstractly, i.e. their representation as polyhedra do not determine their motion properties. In 
particular, some values of the joint parameters may cause overlap of adjacent links. 
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Configuration Space and (7s pan Obstacles 


and orientation of adjacent links, A, and A i+ 1 , is determined by the i ih joint parameter (angle) (26). 
The set of joint parameters of a linked polyhedron completely specifies the position and orientation of 
all the links. 


5. Configuration Space and Cspace Obstacles 

The configuration of a /c-dimcnsional polyhedron, A, is a point a — ( 71 ,..., 7 ,/) G with d — 
k + ( 2)1 where ( 71 ,, 7 c)is the position of rv A and ( 7 c+j, ■ ■ ■, 77 ) are the Ruler angles specifying 
the orientation of A relative to its initial orientation. The configuration of a linked polyhedron having 
d joints is the d-vector of die joint parameters. The d-dimensional space of configurations of A is 
denoted Cspace A . A in configuration x is (/!)„; A in its initial configuration is (/1) 0 . 

The fundamental observation about Configuration Space is that, in Cspace A , die (A) x is repre¬ 
sented by the vector x. Given this, the basic problem in the Cspace approach to spatial planning is to 
define how the obstacles B, map into Cspace 1 . The mapping chosen here exploits two fundamental 
properties of objects: Their rigidity , which allows their configurations to be characterized by a few 
parameters, and their solidity, which requires that a point not be inside more dian one object. 

Definition: The Cspace A obstacle due to D, denoted CO A (B), is defined as follows: 

CO a (B ) = {iG C space A | ( A) x f)B jA 0 } 

finis, if x £ CO a [B) dien (A) x and B eidier touch or overlap. Conversely, any configuration 
x ^ CO A (X) (for all obstacles X) is safe. The following defines a Cspace A entity complementary to 

co A (B). 

Definition: The Cspace A interior of B, denoted CI A {B), is defined as follows: 


CI a (B) = {i£ Cspace A \ [A) x CB} 


The sets CO A (B ) and CI A (B ) are difficult to compute and manipulate since they are curved, 
6 -dimcnsional objects when A and B are polyhedra. Instead, diis paper will deal with projections 

of slices and cross-sections ' 5 of CO A [B). For example, for fixed Euler angles of A, the Cspace A 
5 A cross-section is a slice whose lower and upper bounds are equal. 
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ITic Sliding Algorithm for CO xy 


obstacle due to B is denoted CO^ /z (B) indicating that it is a set of xyz positions, rather than the 
full configurations. CO*^~[B) is the projection onto the x,y,z cordinates of an Filler angle cross- 
section 6 In general, the superscript to CO and Cl will indicate the composition of their members, 
e.g. CO'^(B) and CO^°(B) denote sets of {x, y) and (x, y, 0 ) values respectively. 


6. Hie Sliding Algorithm for CO xy 


This section presents a "naive" algorithm for computing CO"‘{B) when A and B are convex 
polygons and the orientation of A is fixed. In subsequent sections, more efficient algorithms are 
presented for this case. 

When die orientation of A is fixed, the configurations of A are simply the positions of the 
reference vertex rv,,\. Clearly, the boundary of CO \(B) is the locus of rv A where A just touches B. 
This suggests a simple algorithm for computing CO T x !J (B ): slide A around the perimeter of B and 
trace the path of rv A . 'flic term sliding is merely suggestive; in practice, knowing which vertex first 
touches an edge completely defines the path of rv A over that edge. 


The central step of this Sliding algorithm is to determine what point (or edge) of A first contacts 
each edge of B and vice versa , Figure 7. The normal vector of each edge of B defines an approach 
direction for the edge. If A is mov ing from a great distance towards an edge of B along the normal 
direction, and no edge of A is parallel to one of B, then contact will first happen at a vertex of/4. This 
contact vertex is the one with the minimum perpendicular distance to die edge of B. As this vertex 
"slides” along die edge of B, rv A traces an edge of CO% J (B) which is parallel to the edge of B and 
of equal length. But this edge is displaced by the distance from the contact vertex to rv A , projected 
along the normal to the edge of B. Interchanging die roles of A and J9 shows that each edge of/4 and 
some vertex of B gives rise to an edge of CO* y (B). This new edge is traced out by rv A as the edge 
of A "slides" along die contact vertex of B; therefore, the new edge is parallel to die edge of A and 
of equal length, but displaced from the contact vertex of B by the perpendicular distance between the 
edge and rv A . If A and B have pairs of parallel edges, then CO :r ^(B) will have a parallel edge for 
each such pair, displaced as above, but whose length is die sum of the two edges. 

6 When d's orientation is fixed, we assume without loss of generality that A is in its initial orientation, i.e. A is simply 
(A)o displaced by some 3-vector x. 






The Sliding algorithm needs 0{\edges{A)\ X \edges(B)\) operations to compute CO^(B) for 
convex A and J3. This is not optimal; Section 8 describes an 0{\e.dges{A)\ -f- \edges(B)\) algorithm 
for this task. 

The Sliding algorithm derives the edges of CO^ y (B) from the interaction of one of (a) an edge 
of B and a vertex of A, (b) an edge of A and a vertex of B, or (c) an edge of A and an edge of 
B. Similarly, each face of CO% yz (B), for A and B convex polyhedra, can be computed from the 
interaction of one of 

a. a face of A and a vertex of Bor a face of B and a vertex of A, 

b. a face of A and a coplanar edge of B or vice versa. 










14 Vector Set Sums and Configuration Spare Obstacles 



Figure 8 . Illustration of the Proof of Theorem 2. Any location of rv A . in this case e, for which A and B have a 
point in common (expressible as b and o'), can be expressed as c — b — u'. Therefore, CO%>*(B) = BQ(A ) 0 . 


c. an edge of A and a non-parallel edge of B, 

d. a face of A and a coplanar face of B. 

7. Vector Set Sums and Configuration Space Obstacles 

The fundamental result of this section is the following: 

Theorem 2: For A and B, sets in 92 3 , CO A !Z {B) — B © (A)q. 

Proof: If c is an (x, y,z ) configuration of A then (A) c — c © (/4) 0 . Therefore, if a £ ( A) c then 
a — a'-j-c, where a' £ (A)q, sec Figure 8. If£> £ Bf)[A) c , then 6 = a!-\-c and therefore c = b — a'. 
Clearly, the converse is also true. 

I 

If A and B are convex then A ® B and A © B are also convex [33 p.9], therefore CO*^ z (B) is 
convex. Also, for B and A in their initial configurations, 

COr({A) o) - ((>4)o © (B) o) = eCOri(B)o). 
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Vector Set Sums and Configuration Space Obstacles 



Figure 9. Characterization of CJ xy in terms of set addition. The outermost polygon is B , the innermost is 
Cr^ J (B). 'Hie dashed polygons are copies of ( t)<> placed at vertices of B , therefore the convex hull of the 
inner polygon and these dashed polygons is by Theorem 4. Note that Cl (&)($) (A) q ^ B, 

the shaded regions arc B — (C1^ J (B) ® (/t) 0 ). 

A related characterization of CI^ JZ (B) is also possible. For c an (x, y, z ) configuration, if c E 
CIT(B), then c © (/!)„ C B. Since CF^{B) is the set of all such c, then CI%*(B) © (A ) 0 C B 
and furthermore CI% JZ {B) is die maximal such set, sec (a) and (b) below. Clearly, if£ = X © (A) 0 
then X — CI°£' Z (B), sec (c). Figure 8 illustrates these results. 

Theorem 3: For/! and B convex polyhedra, 

(a) VX: X © (A)o C B « X C CI^ Z (B) 

(b) CI f z {B) © (A) 0 C B 

(c) CI^(CO^X(B)) = B 

Note also that CIa(B) — — COa( — B ), i.e. for A to be inside B, it must be outside of B’s 
complement. 
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8. Algorithms for CO^(Ei) and (700(2?) 


Theorem 4 provides a way of computing exactly for convex A and B. In addition, it 

provides an approximation technique for CO^ JZ (B) when A and B arc non-convex. 

Theorem 4: For polyhedra A and 23, 

conv{A 0 23) = conv(A) 0 conv(B) — conv(yert(A ) 0 vert(B )) 

where conv(X) denotes the convex hull 7 of set X and vert(X) is the set of vertices of X. 

Proof: First show that conv{A 02?) = conv(A) 0 conv(B). 

Q) 

The definition of convex hull states that any a £ co7iv(A) can be expressed as an affine combina¬ 
tion of points in A. This may also be done for any b £ conv(B). If x £ conv(A) 0 conv{B), 
a £ conv{A ), b £ conv{B), a { £A,bi £ B, 7 , = 1 , 7 ; > 0, = 1 and /3j > 0 then 

x = a + b — (Jlft) + b — + b ) 


X + X! Wj) = X^fX 

r A A V A 


Pi a i ~h 


2>a) 

A * 


X ^ X ^( a i + b j) — X X + b j) 


7 3 


hut, since — 1 and 7 ,/?, > 0, x is an affine combination of points in A 0 B and 

therefore belongs to its convex hull. Therefore conv(A) 0 conv(B ) C conv(A 0 B). 

(C) 

If x £ conv(A 02?), then for a, £ A and 6 ; £ B, 


x 


X T*( a i + b i) — X T» a * + X TA- 


Therefore, x £ conv(A) 0 conv(B). 

This establishes that conv(A 0 2?) 
for 2 ? and using the fact that conv(A ) = 
conv(A) 0 conv{B). 

I 


= conv(A) 0 conv(B). Replacing vert(A) for A and vert(B ) 
conv(vert(A)) [13], shows diat conv(vert(A) 0 vcrt(B )) = 


7 The convex hull conv(A), of a nonempty set A C is { Y' n , 
1,2,...} [13 p.15] 


7 iXi x 


G A,% > 0, 23”==! ^ = Fn 
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Algorithms for CO^ yz (!l) and CO^ J (R) 


Corollary: For convex polyhedra A and B, A © B — conv(vert(A ) 0 veri(B)) and therefore 
CO J ’( )Z (B) — conv{x>ert{B ) © vert[(A)o)). 

Proof: The first part of the corollary follows directly from the fact that, for convex A, A — 
conv(A). The second part follows from Theorem 2. 

I 

Many algorithms exist for finding the convex hull of a finite set of points on the plane, e.g. [9] 
[12] [15] [29], [29] also describe an efficient algorithm for points in 5R 3 . These algorithms are known 
to run in worst-case time 0(v log v), where v is the size of the input set. Therefore, Theorem 4 leads 
immediately to an algorithm for CO ryz and an upper bound on the computational complexity of the 
problem. 

Theorem 5: For convex A, B C 5? 3 , CO x J JZ (B) can be computed in time 0(n 2 log n); where 
n — \vert(A)\ 0 \vert{B)\. 

Proof: The set vert(B) © vert[(A) o) is of size |oer£(/l)| X \vert[B)\, i.c. 0(n 2 ). Applying an 
0(v log v) convex hull algorithm to this set gives an 0(n 2 log n) algorithm for computing CO(( JZ (B). 
This result holds only for convex polyhedra of dimension k < 3 [7], 

I 

The algorithm of Theorem 5 is not optimal; an 0(n) algorithm exists for CO^'{B) when A and B 
are convex polygons 8 : 

Definition: 7r{0 u) denotes the supporting plane (line) of A with outward normal u. -n{A, it) 
contains at least one boundary point of A, call it a, and for any a' £ A then (a 1 , u) < (a, it). Thus, all 
of A is in one of the closed half spaces bounded by 0/1, it) and it points away from the interior of A. 

Lemma 1: If A and B are convex sets then 

■k(a © b, «) n {A © B) = (0/i, it) n a) © (0b, u) n b) 

8 The development in this section is based on that in Section 14 of [4] 


(i) 
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Figure 10. Illustration for Lemma 1. 

Lemma 2: 

(a) J.et s(a h a 2 ) be a line segment and b a point, then s(oi,a 2 ) 06 = s(aj + b, a 2 0 b) is a line 
segment parallel to s(ai, a 2 ) and of equal length. See Figure 11(a). 

(I)) 1 .et s(< 2 |, a 2 ) and s(6 1 ,6 2 ) be parallel line segments such that (a 2 — ai) = k{b 2 — bj) for 
k > 0. Then s(ai, a 2 ) 0 s(6|, 6 2 ) = s(aj 0 b lt a 2 + 6 2 ) and the length of the sum is tire sum of the 
lengths of lire summands. See Figure 11(b). 

Theorem 6: For convex polygons A and B, CO^’{B) can be computed in worst case time 
0{\vert{A)\ + |uerf(jB)|). 

Proof: For fixed u, each term on the right hand side of (1) is either a line segment (edge) or a 
single point (vertex), it follows from Lemma 2 that the term on the left is one of: 

a. a new vertex, when two vertices are combined; 

b. a displaced edge, when an edge and a vertex are combined (Lemma 2a); 









Figure 11. Illustration for Lemma 2. 


c. a pair of displaced end-to-end edges, when two edges are combined (I emma 2b). 

As u rotates counterclockwise, the boundary of A ® B is formed by joining a succession of these line 
segments. Note that, because of the convexity of A and B, each edge is encountered exactly once [22 
p.13], 

A polygon is stored as a list of vertices in the same order as they are encountered by the coun¬ 
terclockwise sweep of u. This is equivalent to a total order on the edges, based on the angle that the 
edge makes with the x axis. For a polygon P, assume the j th edge in this order, e 3 — s(vj, u J+1 ), 
makes the angle Oj, then 

(v 3 , if 0j_i < 0(u) < 9j 

K(P, u) C\P — { Cj, if 0[u) — 0j 

v j+h if 0j < 0{u) < 0 j+ i 

The time for constmcting tlie new vertices is bounded by a constant, since it involves at most two 
vector additions. Thus A ® B can be computed in linear time during a scan of the vertices of A and 
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B , Figure 12. An implementation of this operation is shown in Appendix 1. Similarly B© {A ) 0 can be 
computed in linear time by first converting each vertex a, : to rv A — a,, Figure 12. 

I 


When A and/or B arc non-convex polygons, CO^(B) can be computed by an extension of 
the algorithm above. The method relies on decomposing the boundaries of the polygons into a 
sequence of polygonal arcs whose internal angles, i.c. the angle facing the inside of the polygon, 
are each less than 7r. The algorithm of Theorem 6 can then be applied to pairs of arcs; the result 
is a polygon whose boundary, in general, intersects itself. The algorithm requires, in the worst case, 
0(\edges(A)\ X \edges(B)\) steps [20], 
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9. Dealing with Rotations of A 


If A and B are polygons, then CO A (B) is an object in 3^, denoted CO xy0 (B). The shape of 
CO a j0 (B ), when A and B are convex, will be investigated below by examining changes in the cross- 
sections 9 of CO x ,^° as 0 changes. 

Assume a fixed value for 0. If A and B have no parallel edges, die proof of Theorem 6 shows that 
each edge of CO" 1 (B) can be expressed as one of: 


e 'l = bj ® s(a;(0), a i+i (0)) (2a) 

€> j — a >{@) ® s {bj,bj+ 1) (2b) 

In these expressions, bj is the position vector of the j lh member of vert(B) and a,(ft) is the position 
of the i th member of vert(Q(A\f), which depends on 0. The order in which die a { and bj are 
encountered in the counterclockwise scan described in Theorem 6 determines the (i,j) pairings of 
vertices and edges. 


Equation (2) shows that, for small changes in 0, the e? rotate around b } , while the e 1 ) are simply 
displaced. Figure 13. In addition to these changes in die xy-cross-section of CO^ j0 , there are discon¬ 
tinuous changes at values ofd, denoted 0*, where an edge from A becomes parallel to one from B. For 
values of 0 just greater than these 0*. this pair of edges has a different order in the scan of Theorem 
6 from what they had when 0 was just less than 0*. Therefore, die (i,j) pairings between edges and 
vertices changes. There are 0(\edges(A)\ X \edycs(B)\) such 0* \nCO A y0 . 

Between discontinuities, die lines defined by e b edges have a simple dependence on 0. The edge 
s ( b j, bj+i) is on a line whose vector equation is: (x,Uj) — (bj,u 3 ) where u 3 is die constant unit 
normal to s(bj, Let a,(0) make the angle 0 + y, w'ith the x axis, with constant, and uj make 
die angle 4>j with the x axis. Then, the equation for the line including e b - is 


(uj, x ) (iij, a i(0) -j- bj) 

= ll a tll cos i° + V, — <t>j) + (bj, u 3 ) 

The terms arc illustrated in Figure 14. This equation holds only fort? G \4>j — — y,] between 

discontinuities; but within that interval it defines a plane in the space (x,y,cos0). This space is 
analogous to that defined by semi-log graph paper. As long as die 0-interval is known and cos -1 is 
9 The cross-section of CO I A ye (B), for constant 0, is CO A y (B). . 







Figure 13. CO T r y (B) is the convex hull of the union of vr r/(0(1),, j ) for each b-„ E vert(B). When A 


(and 0(1 )o) rotates by 0 , the e” rotate around bj and the Cj are displaced. When an e" is aligned with 
an (j for some 0, any extra rotation will ittterchange the order in which they are encountered during the 


counterclockwise scan of Theorem 6. 


single valued over the interval then the mapping from ( x, y, cos 0) to the ( x, y, 0) space is unique. The 
0-ranges can always be chosen so that this is the case. 

The e' 1 cannot be treated in a similar fashion because the orientation of the edge changes with 6 , 
i.e. the equation of the surface involve products of the form treos 0 and ycos0. Instead of trying 
to represent them exactly by non-planar surfaces, this section develops a simple approximation tech¬ 
nique that avoids dealing directly with these edges. The technique will be illustrated first for CO xy 
and later for CO xyS . 

For fixed 6, if the lines defined by the eare extended until they intersect, the resulting figure 
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Figure 14. Illustration of terms in equation (3). 


completely includes CO% J (B). This approximation can be very poor when the angle defined by ad¬ 
jacent e'‘ is very acute, sec Figure 15. The approximation can be improved by introducing additional 
lines whose normals point between those of e b J _ l and e’j. These lines should be farther from bj than 
any of the e“ paired with bj in (2a). 

This method can also be used to approximate die slice of CO%’°{B) between discontinuities. The 
e b and the lines to bound die e° edges both define planes in ( x, y, cost?) space, see (3), for some range 
of 0-values. The boundaries of the 0-intervals also define planes whose equations are of the form 
0 ~ <t>j — hi- These planes bound half-spaces whose intersection defines a convex polyhedron in 
{x, y, cos0). This polyhedron contains all of CO x ^° within die 0-interval. Therefore, it can be used 
to approximate CO^ J ° over that interval. The union of the resulting convex polyhedra for each 0- 
interval is an approximation to CO^ B , 

The discussion above shows how to build a set of n = \edges{B)\ -f \vert(B)\ half spaces 
bounding CO% J °(B) within each 0-range. These half spaces can be intersected to construct a convex 
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polyhedron in 0[n log n) time [7] [30], There are 0(\edges(A)\ X \edges(B)\) 0- ranges that need to 

< 

be considered; therefore, the complete approximation may be found in 0(n 3 log n) time, although in 
many applications, a complete approximation might not be necessary. 

The same techniques can also be applied to computing CI X ( ,0 (B) and since the Cl only has edges 

of the form (26), the resulting polyhedra in (x, y, cost?) arc an exact representation of the CspaceA 
entity [20], 

This approximation is, in principle, applicable to polyhedra in 3-dimensions; the results would be 
a set of polyhedra in 6-dimensions. The extension is conceptually simple, but die difficulties of deriv¬ 
ing and representing die direc-dimcnsional orientation constraints make die approach unattractive. 
The next section examines an alternative to dealing with die high dimensional polyhedra required by 
this technique. 


10. Approximating High Dimension Cspace Obstacles 

Section 3 introduced die use of projections of obstacle slices as a means of approximating high 
dimensional Cspace Obstacles. Figure 16 shows a decomposition of CspaceA C !R 3 into a family 
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of slices Cspace,i[7j,7j +1 ]. I each of these new Cspace s is a 2 slice projection of Cspace A . This 
new family of spaces is a conservative representation of the obstacles, i.e. it represents the worst case 
constraints on configurations of A whose z coordinates arc within the range [7^ 7y-f 1]. 

The power of the slice projection approach is that the family of slices captures all the constraints 
needed to plan safe paths in Cspace A . Having all the slices of the CO A [B ,), there is no need to 
refer to the CO,\{B ,) themselves. On the other hand, the algorithms in the Sections 6 through 9 are 
for cross-sections of CO A (B), e.g. CO x p‘ and CO™ 10 , not for slice projection. The basic result of 
this section is that these algorithms can be used to compute polyhedral approximations to die slice 
projections. 

The construction that relates slice projections to cross-section projections is the swept volume of an 
object. Intuitively, the swept volume of/4 is all the space that/4 covers when moving within a range of 
configurations. In particular, given two configurations for/4, called c and c', then the union of(/4) a for 
all c < a < d is the sw'ept volume of A over the configuration range [c, d\. Generally, c and d differ 
only on some subset, K, of the configuration coordinates. For example, if c and d are of the form 
(A, fih A) and K — {3}, then the swept volume of A over the range [c, d] K refers to the union of/4 
over a set of configurations differing only on /?j. The swept volume of/4 over a configuration range is 
denoted A[c, d\x. 

The swept volume of/4, a rigid object, is also a rigid object 10 with the same number of degrees of 
freedom. For linked polyhedra, the situation is not so simple, because of the interdependence of the 
Cspace parameters. 

Note diat for a linked polyhedron, die position of link j typically depends on the positions of 
links k < j, which are closer to tire base dian link j. Let K — {;}, c — (6i), d = (01), and [c, d) K 
define a range of configurations differing on the j th Cspace A parameter. Since joint j varies over a 
range of values, links l > j will move over a range of positions which depend on the values of c and 
c', as shown in Figure 17. The union of each of the link volumes over its specified range of positions 
is the swept volume of the linked polyhedron. The swept volume of links j dirough n can be taken 
as defining a new j th link. The first j — 1 links and die new j th link define a new manipulator whose 
configuration can be described by the first j — 1 joint parameters. On die other hand, the shape of the 

l0 Note that, in general, the swept volume of a polyhedron is not a polyhedron, although the development relies on 
computing polyhedral approximations to it 
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higure 17 . Changes in the second joint angle from O2 to 0 2 causes changes in the configurations of both link 
A 2 and link A3. 


new link j depends not only on the /T-paramcters of c and c', i.e. Oj and 0', but also on 0, for l > j. 

•J 

This implicit dependence on parameters of c and d that are not in K is undesirable, since it means 
that the shape of the new j lh link will vary. Letting K — {j,..., n}, then the shape of the swept 
volume depends only on the /^-parameters of c and d, while its configuration is determined by the 
(/ — K) -parameters. A swept volume that satisfies this property is called displaceable. 

The fact that the swept volume of a linked polyhedron A does not have the same degrees of 
freedom as does A forms the basis for the relationship between slice projection and cross-section 
projection. If the swept volume is displaceable, the I — K parameters may be changed, but changes 
to the K parameters arc not legal. Therefore, the Cspace of the swept volume of A is of lower 
dimension than the Cspace of A. In particular, configurations in Cspace A that have equal I — K 
parameters and whose K parameters are in tine defining range of the swept volume, project into the 
same configuration in C space A ^ r ^ K . 

If A[c,d] K overlaps some obstacle B then, for some configuration a in that range, (A) a overlaps 
B. The converse is also true. If/l[c, d) K is displaceable, then CO a i c ^ k (B) is the set of / — K projec¬ 
tions of those configurations of /I within [c, d] K for which A overlaps/?, Equivalently, CO A[cc ,] K (B) 
is tine I — K projection of the [c, c']/< slice of CO A (B). If the configurations of the swept volume are 
one of ( x, y), (x, y, z\ or (x , y, 0) then tine algorithms of the previous sections can be used to compute 




28 


Related Work in Spatial Planning 


CO t[ (V '] K (-B) and thereby compute the required slice projections 11 . 

A formal statement and proof of this result is included in Appendix 2 as Theorem 8. This theorem 
is of practical importance since it provides the mechanism underlying the Findspacc and Findpath 


implementations described in [20] and [21], In addition, the proof of the theorem demonstrates the 
usefulness of the Cspace concept as a tool in theoretical analyses of spatial planning problems. 


11. Related Work in Spatial Planning 


fhe definition of the Findspacc problem used here is based on that in [46], Approaches to this 


problem are described by [37] and [27], The latter, which is the more relevant, is an application of the 


Warnock algorithm for hidden line elimination. It involves recursively subdividing the workspace un¬ 
til an area "large enough" for the object is found. This approach has several drawbacks: (1) any non- 
overlapping subdivision strategy will break up potentially useful areas, and (2) the implementation of 
the predicate "large enough" is not specified. 


The Cspace approach to Findspacc and Findpath described here is an extension of that reported 
in [21]. In that paper, an approximate algorithm for CO xyz {B) is described and the Vgraph algorithm 
for high-dimensional Findpath is first presented. 

The basic idea of representing position constraints as geometric figures, e.g. CO x ^>(B), has been 
used (independently) in [1], [2] and [3], who employed an algorithm to compute CO xy for non-convex 
polygons in a technique for two dimensional layout. The template packing approach described in 
[10] uses a related computation based on a chain-code description of figure boundaries. [36] reports 
algorithms for packing of parallelopipcds in the presence of obstacles using a construct equivalent to 
the CO xy , but defined as "the hodograph of the Close Positioning Function". The only use of this 
construct in the paper is for computing CO xy for aligned rectangular prisms. 

The work by Udupa, reported in [40] [41], was the first to approach Findpath by explicitly using 
transformed obstacles and a space where the moving object is a point. Udupa used only rough ap¬ 
proximations to the actual Cspace obstacles and had no direct method for representing constraints on 

"Of course, this requires computing a convex polyhedral approximation to the swept volume of A. Simple approximations 
are not difficult to compute [20], but this is an area where better algorithms are required. Nevertheless, the swept 
volume computation is a 3-dimensional operation which can be defined and executed without recourse to 6-dimensional 
constructs. 
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more than three degrees of freedom. [41] also surveys previous heuristic approaches to the Findpath 
problem for manipulators [17] [28] [44], An early paper on Shakey [25] describes a technique for 
Findpath using a simple object transformation that defines safe points for a circular approximation 
to the mobile robot and uses a graph search formulation of the problem. More recent papers on 
navigation of mobile robots arc also relevant to 2-dimcnsional Findpath [11] [23] [39], [14] reports on a 
program for planning the path of a 2-dimensional sofa through a corridor. This program does a brute- 
force graph search through a quantized Cspace. 

[31] proposes an extension of the approach in [21] to the general Findpath problem, but using 
an exact representation of the high-dimensional Cspace obstacles. The basic approach is to define 
tire general configuration constraints as a set of multinomials in the position parameters of A. But, 
the proposal still requires elaboration. It defines die configuration space constraints in terms of the 
relationships of vertices of one object to die faces of die other. This is adequate for polygons, but 
the equations in die paper only express the constraints necessary for vertices of/4 to be outside ofB, 
i.e. they are of the form of (3) above. They do not account for the positions of A where vertices of 
B are in contact with A. Thus, the equations do not represent the correct constraints on the position 
of A. The new equations will have terms of die form a: cost? and ycosO. Furthermore, the approach 
of defining the configuration constraints by examining die interaction of vertices and faces does not 
generalize to 3-dimcnsional polyhedra. It is not enough to consider die interaction of vertices and 
faces; die interaction of edges and faces must also be taken into account [6]. 
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Appendix 1. Algorithm for CO x ^(B) 

This appendix shows an algorithm for A 0 B, SET-SUM{A, B), when A and B are convex 
polygons. Section 5 shows how this operation can be used to compute CO xy . 

Rach polygon is described in terms of its vertices and the angles that the edges make with the 
positive x axis. The edges and vertices are ordered in counterclockwise order, i.c. by increasing angle. 

1 he polygon structure in die following program has die following components: 

1. size — number of edges in polygon. 

2. vert [0: s ize] — an array of vectors representing the coordinates of a vertex. The 
i th edge, i = l,...,size, has the endpoints vert[i-l] and vert[i]. Note that 
vert[0] = vert[size]. 

3. angle [0:size] — the angle that the normal of an edge makes with the x axis, 
monotonically increasing. For convenience angle[0] = angle[size]. 

1 he algorithm follows directly from Theorem 6, noting that in dais representation of polygons, 
edges are represented by successive vertices. 

SET-SUM (a, b) 

{ c = new-polygon (a.size + b.size); /* create new polygon of max size */ 
ea = 1; eb = 1; vc = 0; ang = 0; offset = 0; 
do { ea = ea + 1 } 

until (a.angle[ea] >= b.angle[l] 

and a.angle[ea - 1] <= b.angle[l]) 
c.vert[0] = a.vert[ea] + b.vert[0]; 
do { vc = vc + 1; 

ang = offset + a.angle[ea]; 
if (ang <= b.anglefeb]) 

then if (ea > a.size) /* handle wraparound */ 

then { offset = 2pi; ea = 1; } 
else ea = ea + 1; 
if (ang > = b ,angle[eb]) 
then eb = eb + 1; 

c.vertfvc] = a.vert[ea] + b.vert[eb]; 

> 

until (ea = a.size and eb = b.size); 
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l = < * K={2 } 


Figure AI8. Illustration of the definition of 4>; s (c, c') and Q^(c,c / ). 


Appendix 2. Proof of Theorem 8 

Assume that Cspace A C let I = { 1 , 2,..., d) and K C I. 7, /< and I — K shall denote 
sets of indices for the coordinates of a E Cspace A . Define the following vectors, all in Cspace A : 
b = {Pi), c = (ji) and d — ( 7 ') for i E 7. Then, 

= {6 G | !\i k <p k <i k ) 

ksK 

$k(c) = <I)/<(c, c) 

©k(c, </) = 4>k(c, o') n $i—k{c, C ) 

These definitions are illustrated in Figure A18. 

The projection operator , denoted P /c [ • i-r 32^1 is defined, for vectors and sets of vectors, by: 

PK[b) = {P k ) kEK 
P K \B) = {P K {b]\bEB) 
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Superscripts on vectors indicate projection, e.g. b K — P K \b). In addition, the vector in 51^1 com¬ 
posed from one vector in and one in I is denoted ( a I ~ K : b K ), where P ; _ A [ (a ! ~ K : b K ) ] 

a /—/ ' and P/<[(a /— K '-b K )] = b K . 

In this notation, precise definitions for the notions of cross-section projection and slice projection 
can be provided. The cross-section projection of a Cspace A obstacle is written as follows: 

CO a (B){c) k == Pj_ K \ CO a (B) n * K {c )]• 

The slice projection, is similar to the cross-section projection, but carried out for all configurations 
between two cross-sections: 

CO a (B)[c, c') k = P,_ K [ CO a (B) n *k(c, c')\. 

The /^-parameters of the two configurations, c and c', define die bounds of the slice. Similarly, the 
swept volume can be defined in this notation: 

Definition: The swept volume 12 of A over the configuration range [c, d ] K is 

{A\c,J) k ) c ~ (J {A) a 

a^B K {c,d) 

The requirement discussed in Section 10 that the swept volume of A be displaceable is embodied 
in die following condition: 

Va: (J {A}^ 1 -* :x><) — {A[c,c’]K\ a i-K :c x) (4) 

arG©/<(c,c / ) 

Note that the I — K parameters may be changed, as in (4), but not diose parameters in K. Therefore, 
(A[c, c'] k),i is defined only if a £ $k(c). 

Lemma 3: If (4) holds, i.e. if the swept volume is displaceable, then 

p,- K {co A[C ' C , ]K {B) n $k( c )] = P)—k\{co a {b) 0 ©/<(o, e - c)) n <Mdl- 


12 The similarity in the notation between swept volume and slice projection does not imply any direct relationship. 
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Proof of Lemma: 

( 2 ) 

It a G Pj—k\(CO a (B)Q ©a (0, d —c)) Pi 4>/,(c) ], then there exists an (a l ~~ h : if) £ CO A (B) 
and an x 2 E ®i<( 0, d — c) such that if — if = c K . This implies diat x\ E $k(c, d). Then, using 

( 4 ), 

( y 4)(o f—,< :x i) — (-^4 [c> C ]h')(cd ~K : c K y 

But since (a l ~~ K : if) E CO A (B), then (A)( a I - K :x?) intersects B; therefore, its supersets also inter¬ 
sect B. By the definition of CO 


(a ! H -c h )E CO A[v<d]K (B) n $ k ( c ) => a E Pr—h\CO A [ c y} K (B) n 4>/ t (c)]. 


(C) 


Assume a £ Pj_ K [CO A[CiCl]l< (B) D <I> A (c) ]; then, 


(A [ C, d ]/c )(a l -x : c K ) Ft B jA 0 


For all i £ 0/e(O, d — c), ( a ! K : c K ) -f i £ < J>/<(c, d) and P/_/<[ (a ! ^ K : c K ) -f- a;] = a l ~~ K . 
Therefore, by (4) 

{A)(a'-K :c K )+x C (A \c,d ]/e)( a /-K : C K). 

Since these arc all the sets that make up die swept volume, at least one of them must also overlap B 
when die swept volume does; therefore diere must be some x\ £ 0 K (O, d — c) for which 


(A)( q /-k . c h-) +Xl O B yA 0. 

By die definition of CO, 

(a 1 K : c K ) -}- ii £ CO a (B) <=> {a ] ~ K :c H ) £ ( CO A [B ) — 0/c(O, d — c)) Pi $/<(c). 
Clearly, then 

a £ Pj^KCO^B) - © K (0, d - c)) D <t> K (c) ] 

I 

This Lemma leads to a proof of Theorem 8. 
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Theorem 8: If(4) holds, then 

f . ■ ■ 

Pi—k[co a {b) n <Mc, c f ) } = Pi-k\co a[C}C , ]k {b) n <M<0 ] 

Proof of Theorem: 

The proof below shows that 

Pi-k [ CO x {B) n $k{c, d) ] - Pi- K [(CO a {B) © ©a(0, d - c)) n */c(c)j. 

1'hc theorem follows directly from this result and Lemma 3. 

(C) 

If a G Pi—k[ CO.\{B) D $k[c, d) ], then, for some x £ ©a (0, d — c) there is an ai such that 

a { = (o'-* : c* + x }< ) £ CO A (B) f) <f> K (c, c') 

Since x /_A = 0, then a[ — x £ 4 >a (c) and therefore 

a, - x £ (COa(B) © ©a ( 0 , d - c)) n (c). 

Since a = P/_/ v -[aj — a:] 

a £ Pj —a [ (COn©) © ©a (0, d - c )) 0 *k(c)]. 

Q) 

Assume a £ P/_ a [ (C'0,, 1 (B) © © /v (0, c 7 — c)) n $a(c) ], then, 

W ~~ K : c K ) £ (CO r(e) © ©a(0, d - c)) 0 <M4 

Since the / — A" parameters arc not changed by the set subtraction, there must be an aj = 
(a /~ K : xf) £ CO a (B). x\ must be in $ K (c, d), because Pk[x\ — x 2 ] — c with x 2 £ ©/c(0, d — c). 

{a ! - K : xf) £ CO a {B) D d) =* a £ Pj_ K [ CO A {B) © $/c(c, ©]. 





